﻿using System;
using System.Diagnostics;

namespace Mapenect.KinectInterface
{
    class MotorControl
    {
        private string _devSerial;          // Device Id
        private IntPtr motor = IntPtr.Zero; // Pointer auf Motorgerät
        private short motorPos = 0;         // Position des Motors

        internal bool Initialize()
        {
            try
            {
                _devSerial = CLNUIDevice.GetDeviceSerial(0);
                motor = CLNUIDevice.CreateMotor(_devSerial);
                CLNUIDevice.SetMotorPosition(motor, motorPos);
                CLNUIDevice.SetMotorLED(motor, (byte)1);        // Einschalten der LED -> 0:aus, 1:grün, ...
                return true;
            }
            catch (Exception ex)
            {
                Debug.WriteLine(ex.Message);
                return false;
            }
        }

        internal bool MoveUp()
        {
            try
            {
                motorPos = (motorPos + 1000) > 8000 ? (short)8000 : (short)(motorPos + 1000);
                CLNUIDevice.SetMotorPosition(motor, motorPos);
                return true;
            }
            catch (Exception)
            {
                return false;
            }
        }

        internal bool MoveDown()
        {
            try
            {
                motorPos = (motorPos - 1000) < -8000 ? (short)-8000 : (short)(motorPos - 1000);
                CLNUIDevice.SetMotorPosition(motor, motorPos);
                return true;
            }
            catch (Exception)
            {
                return false;
            }
        }

        internal bool ResetPosition()
        {
            try
            {
                motorPos = 0;
                CLNUIDevice.SetMotorPosition(motor, motorPos);
                return true;
            }
            catch (Exception)
            {
                return false;
            }
        }

        internal bool DisbandMotor()
        {
            try
            {
                CLNUIDevice.SetMotorLED(motor, (byte)0); // LED ausschalten
                this.ResetPosition();
                return true;
            }
            catch (Exception ex)
            {
                Debug.WriteLine(ex.Message);
                return false;
            }
        }
    }
}
